ボール＆ビーム制御実験

オブジェに飾っておくのに面白いものが簡単にできないか考えて，SI2012で見たものを更に簡単にして作ってみました．
ボール＆ビームというのは制御分野ではメジャーな例題らしいですが，自分は専門外なので知りませんでした．

作ったのはこれ↓

Arduinoでサーボを制御，測距センサでピンポン玉との距離を計測，身近にあったスチロール板を適当にホットボンドでくっつけて工作．センサの高さと距離は輪ゴムで固定しているので可変できます．
本体製作時間は１時間半くらい，以下の動画のプログラムも含めて３－４時間で出来ました．

動画その１（ON-OFF制御）
動画その２（比例制御）
動画その３（比例と微分制御）

完全に安定しないのは，測距センサの値がふらつくためです．
完全に安定するとボールが止まってしまって面白く無いので，あえてそのままにしています．
測距センサの計測周期に合わせて別周期で計測とスムージングした値を使うと多分もっと安定すると思います．

－－－－－－－－－－途中だけど，スケッチはこんな感じ－－－－－－－－－－
#include < Servo.h>

const int analogInPin = A0;
int servoCenter = 85;
int sensorValue = 0;
int posCenter=256;
float p=0, pp=0, i=0, d=0, pid=0;

Servo myservo;

void setup()
{
myservo.attach(9);
myservo.write(servoCenter);
delay(1000);
}

void loop()
{
sensorValue = analogRead(analogInPin;
pp=p;
p=-(sensorValue-posCenter);
i+=p; // I制御は使ってません
d=p-pp;
pid=p*0.04+i*0+d*0.04; // I制御は使ってません（係数０）
myservo.write(servoCenter+pid);
delay(200);
}

-----------------------------------------------------------------
更に改良

昨日の続き．ピタっと止まるようにプログラム組みました．
やったのは，
タイマー割り込みを使ってPSDセンサの計測周期を制御周期と独立にしてスムージングによるノイズ除去．
センサ値の線形化
パラメータの最適化．
ピタっと止まってしまって面白く無いので，２通りの位置で１０秒毎に切り替えるようにして動きを出しました．

その動画→ その４（２点の交互移動）

--------------　改良したスケッチ　--------------

#include < Servo.h>
#include < MsTimer2.h>
#define CENTERVAL 256 // センサ読みの線形化に使う
#define POS1 0.75 // 距離を２通り定義，
#define POS2 1.3 // 中心が１なので，左右で少し離れた２点

const int analogInPin = A0; // PSDセンサの出力をアナログポートにより計測
volatile int sensorValue = 0; // PSDセンサの読み
int servoCenter = 86; // サーボが水平になる値．
float posTarget=POS1; // 目標位置
float posCenter=POS1; // 制御対象を目標位置にゆっくり近づかせるための変数
int cnt=0; // 繰り返しの数のカウント
float p=0, pp=0, i=0, d=0, pid=0; // 制御パラメータ

Servo myservo;

// --------- スムージング関数定義 --------------------
float smooth(float cData, float pData, float factor){
return(cData*factor + pData*(1-factor));
}

// --------- PSDセンサ計測関数，タイマー割り込みで呼び出す -------------
void measPSD(){
sensorValue = smooth(analogRead(analogInPin),sensorValue,0.5); // 計測値はスムージングして使う
}

// ----------------------- セットアップ -------------------------------
void setup()
{
myservo.attach(9);
myservo.write(servoCenter); // サーボを水平に
// Serial.begin(9600); // 制御係数の確認のためにシリアル出力
MsTimer2::set(25, measPSD); // 25ms 周期でタイマー割込みをかけてPSD計測
MsTimer2::start();
delay(5000); // 最初５秒サーボを水平に保つ
}
// ----------------------- メインループ ----------------------------------
void loop()
{
cnt++; if (cnt>50){ // 200ms×50回で10秒毎に位置を切り替えるためのカウント処理
cnt=0;
if (posTarget==POS1) posTarget=POS2; else posTarget=POS1; // 目標位置を切り替える
}
posCenter+=(posTarget-posCenter)*0.15; // 目標位置が急に変化するとバタツクのでなめらか処理
pp=p;
p=(float)CENTERVAL/(float)sensorValue - posCenter; // 比例値
i+=p; // 積分値，今は未使用
d=p-pp; // 微分（差分）値
pid=p*6+i*0+d*20;
myservo.write(servoCenter+pid);

// Serial.println(p);
delay(200); // 制御周期は200ms，早くするとバタついて球が跳ねる
}